#include "main.h"
#include "camera.h"

#include <cmath>
#include <opencv2/opencv.hpp>
#include <vector>

using namespace cv;
using namespace std;

// 计算点到直线的距离
double pointToLineDistance(const Point2f &point, const Point2f &lineStart,
                           const Point2f &lineEnd) {
  double normalLength = norm(lineEnd - lineStart);
  if (normalLength == 0.0) {
    return norm(point - lineStart);
  }
  return abs((point.x - lineStart.x) * (lineEnd.y - lineStart.y) -
             (point.y - lineStart.y) * (lineEnd.x - lineStart.x)) /
         normalLength;
}

// 判断两条线段是否共线
bool areCollinear(const Vec4i &line1, const Vec4i &line2, double threshold) {
  Point a1(line1[0], line1[1]), a2(line1[2], line1[3]);
  Point b1(line2[0], line2[1]), b2(line2[2], line2[3]);

  // 计算两条线段的距离
  double dist1 = pointToLineDistance(a1, b1, b2);
  double dist2 = pointToLineDistance(a2, b1, b2);

  return (dist1 < threshold && dist2 < threshold);
}

// 合并两条共线线段
Vec4i mergeLines(const Vec4i &line1, const Vec4i &line2) {
  vector<Point2f> points = {
      Point2f(line1[0], line1[1]), Point2f(line1[2], line1[3]),
      Point2f(line2[0], line2[1]), Point2f(line2[2], line2[3])};

  // 找到x和y的极值
  float minX = points[0].x, maxX = points[0].x;
  float minY = points[0].y, maxY = points[0].y;

  for (const auto &p : points) {
    minX = min(minX, p.x);
    maxX = max(maxX, p.x);
    minY = min(minY, p.y);
    maxY = max(maxY, p.y);
  }

  // 确定主方向（更长的维度）
  if ((maxX - minX) > (maxY - minY)) {
    return Vec4i(minX, (minY + maxY) / 2, maxX, (minY + maxY) / 2);
  } else {
    return Vec4i((minX + maxX) / 2, minY, (minX + maxX) / 2, maxY);
  }
}

// 计算两条直线的交点
bool computeIntersection(const Vec4i &line1, const Vec4i &line2,
                         Point2f &intersection) {
  Point2f p1(line1[0], line1[1]);
  Point2f p2(line1[2], line1[3]);
  Point2f p3(line2[0], line2[1]);
  Point2f p4(line2[2], line2[3]);

  float denom = (p4.y - p3.y) * (p2.x - p1.x) - (p4.x - p3.x) * (p2.y - p1.y);
  if (denom == 0.0) { // 平行线
    return false;
  }

  float ua =
      ((p4.x - p3.x) * (p1.y - p3.y) - (p4.y - p3.y) * (p1.x - p3.x)) / denom;
  float ub =
      ((p2.x - p1.x) * (p1.y - p3.y) - (p2.y - p1.y) * (p1.x - p3.x)) / denom;

  if (ua >= 0.0f && ua <= 1.0f && ub >= 0.0f && ub <= 1.0f) {
    intersection.x = p1.x + ua * (p2.x - p1.x);
    intersection.y = p1.y + ua * (p2.y - p1.y);
    return true;
  }

  return false; // 线段不相交
}

// 获取线段上离给定点最远的端点
Point2f getFarthestEndpoint(const Vec4i &line, const Point2f &point) {
  Point2f p1(line[0], line[1]);
  Point2f p2(line[2], line[3]);

  double dist1 = norm(p1 - point);
  double dist2 = norm(p2 - point);

  return (dist1 > dist2) ? p1 : p2;
}

// 判断两条线段是否近似垂直
bool isPerpendicular(const Vec4i &line1, const Vec4i &line2,
                     double angleThreshold) {
  Point2f vec1(line1[2] - line1[0], line1[3] - line1[1]);
  Point2f vec2(line2[2] - line2[0], line2[3] - line2[1]);

  // 归一化向量
  double len1 = norm(vec1);
  double len2 = norm(vec2);

  if (len1 == 0.0 || len2 == 0.0) {
    return false;
  }

  vec1 /= len1;
  vec2 /= len2;

  // 计算点积和角度差
  double dotProduct = vec1.x * vec2.x + vec1.y * vec2.y;
  double angleDiff = acos(abs(dotProduct)) * 180.0 / CV_PI;

  // 检查是否接近90度
  return abs(angleDiff - 90.0) < angleThreshold;
}

// 判断两条线段长度是否近似相等
bool approxEqualLength(const Vec4i &line1, const Vec4i &line2,
                       double ratioThreshold) {
  double len1 = norm(Point2f(line1[2] - line1[0], line1[3] - line1[1]));
  double len2 = norm(Point2f(line2[2] - line2[0], line2[3] - line2[1]));

  if (max(len1, len2) == 0.0) {
    return false;
  }

  double ratio = abs(len1 - len2) / max(len1, len2);
  return ratio < ratioThreshold;
}

// 从两条垂直边找到正方形的四个角点
bool findSquareCorners(const Vec4i &line1, const Vec4i &line2, Point2f &p1,
                       Point2f &p2, Point2f &p3, Point2f &p4) {
  // 计算两条直线的交点
  Point2f corner;
  if (!computeIntersection(line1, line2, corner)) {
    return false;
  }

  // 找到每条线上离交点最远的点
  Point2f line1_end = getFarthestEndpoint(line1, corner);
  Point2f line2_end = getFarthestEndpoint(line2, corner);

  // 计算对角向量
  Point2f vec1 = line1_end - corner;
  Point2f vec2 = line2_end - corner;

  // 计算第四个点（对角点）
  Point2f opposite_corner = corner + vec1 + vec2;

  // 确保点的顺序是顺时针或逆时针
  p1 = corner;
  p2 = line1_end;
  p3 = opposite_corner;
  p4 = line2_end;

  // 验证形状是否合理
  double side1 = norm(p2 - p1);
  double side2 = norm(p4 - p1);
  double diagonal = norm(p3 - p1);

  // 检查对角线长度是否符合勾股定理
  double expectedDiagonal = sqrt(side1 * side1 + side2 * side2);
  if (abs(diagonal - expectedDiagonal) > 0.2 * expectedDiagonal) {
    return false;
  }

  return true;
}

int main() {
  init();
  // 初始化摄像头
  cv::VideoCapture cap;
  initCamera(cap, 10, 1);
  createTrackbars();
  cameraMatrix =
      (cv::Mat_<double>(3, 3) << 3486.526926, 0.000000, 675.717747, 0.000000,
       3506.261576, 545.925082, 0.000000, 0.000000, 1.000000);
  distCoeffs = (cv::Mat_<double>(1, 5) << -0.644417, 0.601275, -0.003482,
                0.042929, 0.000000);

  objectPoints = {cv::Point3f(0, A4_WIDTH_M / 2, A4_HEIGHT_M / 2),
                  cv::Point3f(0, -A4_WIDTH_M / 2, A4_HEIGHT_M / 2),
                  cv::Point3f(0, -A4_WIDTH_M / 2, -A4_HEIGHT_M / 2),
                  cv::Point3f(0, A4_WIDTH_M / 2, -A4_HEIGHT_M / 2)};

  cv::Mat frame, origin, gray, binary, undistort;
  while (true) {
    auto start = steady_clock::now();

    cap >> frame;
    if (frame.empty()) {
      static int camera_error_count = 0;
      camera_error_count++;
      if (camera_error_count > 10) {
        cap.release();
        initCamera(cap);
      }
    }

    cv::undistort(frame, undistort, cameraMatrix, distCoeffs);
    origin = undistort.clone();

    // 0  垂直翻转（沿X轴翻转）
    // >0 水平翻转（沿Y轴翻转）
    // <0 水平垂直翻转
    transpose(origin, origin);
    flip(origin, origin, 1);

    cv::imwrite("origin.jpg", origin);

    cv::cvtColor(origin, gray, cv::COLOR_BGR2GRAY);
    cv::threshold(gray, binary, gray_thres1, 255, cv::THRESH_BINARY_INV);

    vector<vector<cv::Point>> contours;
    vector<cv::Vec4i> hierarchy;
    findContours(binary, contours, hierarchy, cv::RETR_TREE,
                 cv::CHAIN_APPROX_SIMPLE);

    vector<cv::Point> a4Contour;
    vector<vector<cv::Point>> filterContours;
    for (int i = 0; i < contours.size(); i++) {
      // find inner a4
      if (hierarchy[i][3] == -1) {
        continue;
      }
      auto minArea = cv::minAreaRect(contours[i]);
      double hwr;
      if (minArea.size.height > minArea.size.width)
        hwr = minArea.size.height * 1.0 / minArea.size.width;
      else
        hwr = minArea.size.width * 1.0 / minArea.size.height;

      std::vector<cv::Point> approx;
      cv::approxPolyDP(contours[i], approx, epsilon1, true);
      if (approx.size() != 4) {
        continue;
      }

      if (hwr > hwr_min / 10.f && hwr < hwr_max / 10.f &&
          minArea.size.area() > area_min) {
        filterContours.emplace_back(approx);
      }
    }

    cv::Mat crop, cropBinary;
    if (filterContours.size() == 1) {
      cv::Point2f verticePix[4] = {
          static_cast<cv::Point2f>(filterContours[0][0]),
          static_cast<cv::Point2f>(filterContours[0][1]),
          static_cast<cv::Point2f>(filterContours[0][2]),
          static_cast<cv::Point2f>(filterContours[0][3])};
      // auto a4MinRect = cv::minAreaRect(filterContours[0]);
      // cv::Point2f vertices[4];
      // a4MinRect.points(vertices);
      std::vector<cv::Point2f> verticeVec = sortRectanglePoints(verticePix);
      cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64F);
      cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64F);
      cv::solvePnP(objectPoints, verticeVec, cameraMatrix, distCoeffs, rvec,
                   tvec, cv::SOLVEPNP_ITERATIVE);
      std::vector<cv::Point2f> projectedPoints;
      cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs,
                        projectedPoints);
      // double D = (cameraMatrix.at<double>(0, 0) * A4_WIDTH_M /
      //                 norm(verticeVec[0] - verticeVec[1]) +
      //             cameraMatrix.at<double>(0, 0) * A4_HEIGHT_M /
      //                 norm(verticeVec[0] - verticeVec[3])) /
      //            2;
      double D = tvec.at<double>(2, 0) + dOffset / 1000.f;
      // cout << "tvec: " << tvec.at<double>(0, 0) << tvec.at<double>(1, 0)
      //      << tvec.at<double>(2, 0) << endl;
      // cout << "rvec:" << rvec.at<double>(0, 0) << rvec.at<double>(1, 0)
      //      << rvec.at<double>(2, 0) << endl;
      auto P = cv::getPerspectiveTransform(verticeVec, a4Vertices);
      cv::warpPerspective(origin, crop, P, cv::Size(A4_WIDTH, A4_HEIGHT));
      cv::drawContours(origin, filterContours, -1, cv::Scalar(0, 255, 0), 2);
      cv::putText(origin, "Dist:" + to_string(D), cv::Point(10, 60),
                  cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);

      for (int i = 0; i < verticeVec.size(); i++) {
        cv::circle(origin, verticeVec[i], 5, cv::Scalar(0, 255, 255), -1);
        cv::putText(origin, to_string(i), verticeVec[i],
                    cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 255), 2);

        for (int j = 0; j < projectedPoints.size(); j++) {
          cv::circle(origin, projectedPoints[j], 5, cv::Scalar(0, 0, 255), -1);
          cv::putText(origin, to_string(j), projectedPoints[j],
                      cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);
        }
      }

      cv::rectangle(crop, cv::Rect2i(0, 0, 10, A4_HEIGHT),
                    cv::Scalar(255, 255, 255), -1);
      cv::rectangle(crop, cv::Rect2i(A4_WIDTH - 10, 0, 10, A4_HEIGHT),
                    cv::Scalar(255, 255, 255), -1);
      cv::rectangle(crop, cv::Rect2i(0, 0, A4_WIDTH, 10),
                    cv::Scalar(255, 255, 255), -1);
      cv::rectangle(crop, cv::Rect2i(0, A4_HEIGHT - 10, A4_WIDTH, 10),
                    cv::Scalar(255, 255, 255), -1);
    }

    if (!crop.empty()) {

      std::vector<result> triangle;
      std::vector<result> rectangle;
      std::vector<result> circle;

      contours.clear();
      hierarchy.clear();
      filterContours.clear();
      cv::cvtColor(crop, cropBinary, cv::COLOR_BGR2GRAY);
      cv::threshold(cropBinary, cropBinary, gray_thres2, 255,
                    cv::THRESH_BINARY_INV);
      cv::Mat element =
          cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
      cv::morphologyEx(cropBinary, cropBinary, cv::MORPH_CLOSE, element,
                       cv::Point(-1, -1), 3);
      cv::findContours(cropBinary, contours, hierarchy, cv::RETR_TREE,
                       cv::CHAIN_APPROX_SIMPLE);

      for (int i = 0; i < contours.size(); i++) {
        // if (hierarchy[i][2] == -1) {
        {
          std::vector<cv::Point> approx;
          cv::approxPolyDP(contours[i], approx, epsilon2, true);
          if (approx.size() > 2) {
            std::vector<std::vector<cv::Point>> approxVec = {approx};
            cv::drawContours(crop, approxVec, -1, cv::Scalar(0, 255, 0), 2);

            if (approx.size() == 3) {
              if (isTriangle(approx)) {
                result tri;
                tri.points = approx;
                tri.x =
                    (norm(approx[0] - approx[1]) + norm(approx[0] - approx[2]) +
                     norm(approx[1] - approx[2])) /
                    3 * 0.5; // unit:mm

                triangle.emplace_back(tri);
              }
            } else if (approx.size() <= 6) {
              cv::Rect rect = cv::boundingRect(approx);
              double aspectRatio = (double)rect.width / rect.height;
              double areaRatio =
                  cv::contourArea(approx) / (rect.width * rect.height);

              // 判断是否为正方形（宽高比接近1且填充率较高）
              if (aspectRatio > 0.7 && aspectRatio < 1.3 && areaRatio > 0.7) {
                result rect;

                cv::Mat mnistBinary = getRectMat(crop, minAreaRect(approx), 28);
                rect.id = digitalDetect(mnistBinary, 28);
                rect.points = approx;
                rect.x = (norm(rect.points[0] - rect.points[1]) +
                          norm(rect.points[0] - rect.points[3])) /
                         4; // unit:mm
                rectangle.push_back(rect);
              }
            } else if (approx.size() > 4) {
              std::cout << "approx verts:" << approx.size() << std::endl;
              // double area = cv::contourArea(approx);
              // double perimeter = cv::arcLength(approx, true);
              // cout << "area:" << area << "perimeter:" << perimeter << endl;

              if (approx.size() == 8) {
                vector<int> hull;
                cv::convexHull(approx, hull, false, false);

                vector<cv::Vec4i> defects;
                cv::convexityDefects(approx, hull, defects);

                vector<int> concave_idx;
                for (const auto &d : defects)
                  concave_idx.emplace_back(d[2]); // 记录所有凹点下标

                if (concave_idx.size() != 2) {
                  cout << "凹点数量不是2，无法正确分组" << endl;
                  continue;
                }

                sort(concave_idx.begin(), concave_idx.end());

                vector<cv::Point> rect1, rect2;
                int n = approx.size();
                int c1 = concave_idx[0], c2 = concave_idx[1];

                // rect2: (c1+1) ~ c2-1
                for (int i = (c1 + 1) % n; i != c2; i = (i + 1) % n) {
                  rect2.push_back(approx[i]);
                }
                // rect1: c2+1 ~ c1-1（可能会跨0）
                for (int i = (c2 + 1) % n; i != c1; i = (i + 1) % n) {
                  rect1.push_back(approx[i]);
                }

                completeRect(rect1);
                completeRect(rect2);

                result rect;
                cv::Mat mnistBinary =
                    getRectMat(crop, cv::minAreaRect(rect1), 28);
                cv::imshow("mnistBinary", mnistBinary);
                int a_id = digitalDetect(mnistBinary, 28);
                rect.id = a_id;
                rect.points = {rect1[0], rect1[1], rect1[2], rect1[3]};
                rect.x = (norm(rect.points[0] - rect.points[1]) +
                          norm(rect.points[0] - rect.points[3])) /
                         4; // unit:mm
                rectangle.push_back(rect);

                mnistBinary = getRectMat(crop, cv::minAreaRect(rect2), 28);
                int b_id = digitalDetect(mnistBinary, 28);
                rect.id = b_id;
                rect.points = {rect2[0], rect2[1], rect2[2], rect2[3]};
                rect.x = (norm(rect.points[0] - rect.points[1]) +
                          norm(rect.points[0] - rect.points[3])) /
                         4; // unit:mm
                rectangle.push_back(rect);
              }

              if (approx.size() == 12) {
                Mat vis = crop.clone();
                vector<Vec4i> lines;
                for (int i = 0; i < approx.size(); i++) {
                  Point pt1 = approx[i];
                  Point pt2 =
                      approx[(i + 1) % approx.size()]; // 连接相邻点形成线段
                  lines.push_back(Vec4i(pt1.x, pt1.y, pt2.x, pt2.y));
                  line(vis, pt1, pt2, Scalar(0, 255, 0), 2); // 绘制原始线段
                }

                // 2. 线段聚类和优化
                vector<Vec4i> mergedLines;
                for (size_t i = 0; i < lines.size(); ++i) {
                  bool merged = false;
                  for (size_t j = 0; j < mergedLines.size(); ++j) {
                    if (areCollinear(lines[i], mergedLines[j],
                                     10.0)) { // 10像素阈值
                      mergedLines[j] = mergeLines(lines[i], mergedLines[j]);
                      merged = true;
                      break;
                    }
                  }
                  if (!merged) {
                    mergedLines.push_back(lines[i]);
                  }
                }

                // 3. 寻找正方形
                vector<vector<Point>> squares;
                for (size_t i = 0; i < mergedLines.size(); ++i) {
                  for (size_t j = i + 1; j < mergedLines.size(); ++j) {
                    if (!isPerpendicular(mergedLines[i], mergedLines[j],
                                         15.0)) // 15度容差
                      continue;
                    if (!approxEqualLength(mergedLines[i], mergedLines[j],
                                           0.4)) // 20%长度容差
                      continue;

                    // 4. 构建可能的正方形
                    Point2f p1, p2, p3, p4;
                    if (findSquareCorners(mergedLines[i], mergedLines[j], p1,
                                          p2, p3, p4)) {
                      vector<Point> square = {p1, p2, p3, p4};
                      squares.push_back(square);
                    }
                  }
                }

                //筛选中间黑色像素多于白色，长宽比接近1的矩形
                vector<vector<Point>> squares_filtered;
                for (const auto &square : squares) {
                  auto rotateRect = cv::minAreaRect(square);
                  double width, height;
                  if (rotateRect.size.width > rotateRect.size.height) {
                    width = rotateRect.size.width;
                    height = rotateRect.size.height;
                  } else {
                    width = rotateRect.size.height;
                    height = rotateRect.size.width;
                  }
                  if (width * 1.0 / height < 1.1) {
                    Mat squareMat =
                        getRectMat(crop, cv::minAreaRect(square), 28);
                    int whitePixels = countNonZero(squareMat);
                    int blackPixels =
                        squareMat.rows * squareMat.cols - whitePixels;
                    if (blackPixels > whitePixels) {
                      squares_filtered.push_back(square);
                    }
                  }
                }

                // 5. 显示结果
                for (const auto &square : squares) {
                  polylines(vis, square, true, Scalar(0, 0, 255), 3);
                }

                // 5. 显示结果
                for (const auto &square : squares_filtered) {
                  polylines(vis, square, true, Scalar(0, 255, 255), 3);
                }

                imshow("Detected Squares", vis);

                for (const auto &square : squares_filtered) {
                  result rect;
                  cv::Mat mnistBinary =
                      getRectMat(crop, cv::minAreaRect(square), 28);
                  cv::imshow("mnistBinary", mnistBinary);
                  int a_id = digitalDetect(mnistBinary, 28);
                  rect.id = a_id;
                  rect.points = {square[0], square[1], square[2], square[3]};
                  rect.x = (norm(rect.points[0] - rect.points[1]) +
                            norm(rect.points[0] - rect.points[3])) /
                           4; // unit:mm
                  rectangle.push_back(rect);
                }

              }

              else if (approx.size() > 10) {
                result cir;
                cir.points = approx;
                cv::Point2f center;
                float radius;
                cv::minEnclosingCircle(cir.points, center, radius);
                cv::circle(crop, center, radius, cv::Scalar(0, 0, 255));
                cir.x = radius; // unit:mm
                circle.push_back(cir);
              } else {
                std::cout << "error" << std::endl;
              }
            }
          }
        }
      }

      for (const auto &contour : triangle) {
        std::vector<std::vector<cv::Point>> apprapproxVec = {contour.points};
        cv::drawContours(crop, apprapproxVec, -1, cv::Scalar(0, 0, 255), 2);
        cv::putText(crop, to_string(contour.x), contour.points[0],
                    cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);
      }
      for (const auto &contour : rectangle) {
        std::vector<std::vector<cv::Point>> apprapproxVec = {contour.points};
        cv::drawContours(crop, apprapproxVec, -1, cv::Scalar(255, 0, 255), 2);
        if (contour.id != -1) {
          cv::putText(crop, to_string(contour.id) + ":" + to_string(contour.x),
                      contour.points[0], cv::FONT_HERSHEY_SIMPLEX, 1,
                      cv::Scalar(255, 0, 255), 2);
        } else {
          cv::putText(crop, to_string(contour.x), contour.points[0],
                      cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 255), 2);
        }
      }
      for (const auto &contour : circle) {
        std::vector<std::vector<cv::Point>> apprapproxVec = {contour.points};
        cv::drawContours(crop, apprapproxVec, -1, cv::Scalar(0, 255, 255), 2);
        cv::putText(crop, to_string(contour.x), contour.points[0],
                    cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);
      }
    }
    auto end = steady_clock::now();

    cv::putText(
        origin,
        std::to_string(duration_cast<milliseconds>(end - start).count()) + "ms",
        cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0),
        2);
    cv::line(origin, cv::Point(origin.cols / 2, 0),
             cv::Point(origin.cols / 2, origin.rows), cv::Scalar(0, 255, 255),
             2);
    cv::line(origin, cv::Point(0, origin.rows / 2),
             cv::Point(origin.cols, origin.rows / 2), cv::Scalar(0, 255, 255),
             2);

    cv::imshow("origin", origin);
    cv::imshow("binary", binary);
    if (!cropBinary.empty()) {
      cv::imshow("cropBinary", cropBinary);
      cv::imshow("crop", crop);
    }

    // 退出键 ESC
    char key = cv::waitKey(1);
    if (key == 27)
      break;
  }

  // 释放资源
  cap.release();
  cv::destroyAllWindows();
  return 0;
}

void init() {
  string model_path = "/home/xsxin/workspace/elec/weight/mnist_verified.onnx";

  net = cv::dnn::readNetFromONNX(model_path);
  net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
  net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
}

void createTrackbars() {
  cv::namedWindow("DEBUG", cv::WINDOW_AUTOSIZE);

  for (const auto &int_param : int_params_list) {
    int_param.value = int_param.default_value;
    cv::createTrackbar(int_param.name, "DEBUG", &int_param.value,
                       int_param.max);
  }
}

// //顺时针TL TR BR BL
// std::vector<cv::Point2f> sortRectanglePoints(cv::Point2f vertices[4]) {
//   int index[4] = {0, 1, 2, 3};
//   if (cv::norm(vertices[0] - vertices[1]) >
//       cv::norm(vertices[0] - vertices[3])) {
//     index[0] = 1;
//     index[1] = 2;
//     index[2] = 3;
//     index[3] = 0;
//   }
//   std::vector<cv::Point2f> sorted(4);
//   for (int i = 0; i < 4; i++) {
//     sorted[i] = vertices[index[i]];
//   }

//   return {sorted[0], sorted[1], sorted[2], sorted[3]};
// }

// 顺时针(TL, TR, BR, BL)
std::vector<cv::Point2f> sortRectanglePoints(cv::Point2f vertices[4]) {
  std::vector<cv::Point2f> points(vertices, vertices + 4);

  // 1. 找到左上(最小x+y), 右下(最大x+y)
  auto sumXY = [](const cv::Point2f &p) { return p.x + p.y; };
  int idx_tl = 0, idx_br = 0;
  float min_sum = sumXY(points[0]), max_sum = sumXY(points[0]);
  for (int i = 1; i < 4; ++i) {
    float s = sumXY(points[i]);
    if (s < min_sum) {
      min_sum = s;
      idx_tl = i;
    }
    if (s > max_sum) {
      max_sum = s;
      idx_br = i;
    }
  }

  // 2. 找到右上(最小x-y)、左下(最大x-y)
  auto diffXY = [](const cv::Point2f &p) { return p.x - p.y; };
  int idx_tr = -1, idx_bl = -1;
  float min_diff = 1e9, max_diff = -1e9;
  for (int i = 0; i < 4; ++i) {
    float d = diffXY(points[i]);
    if (i != idx_tl && i != idx_br) { // skip已分配
      if (d < min_diff) {
        min_diff = d;
        idx_tr = i;
      }
      if (d > max_diff) {
        max_diff = d;
        idx_bl = i;
      }
    }
  }

  std::vector<cv::Point2f> sorted(4);
  sorted[0] = points[idx_tl]; // top-left
  sorted[3] = points[idx_tr]; // top-right
  sorted[2] = points[idx_br]; // bottom-right
  sorted[1] = points[idx_bl]; // bottom-left
  return sorted;
}

bool isTriangle(const vector<cv::Point> &triangle) {
  double a = norm(triangle[0] - triangle[1]);
  double b = norm(triangle[1] - triangle[2]);
  double c = norm(triangle[2] - triangle[0]);

  double maxSide = max({a, b, c});
  double minSide = min({a, b, c});

  // 最大边与最小边之比小于1.3（允许一定变形）
  return (maxSide / minSide) < 1.3;
}

void completeRect(std::vector<cv::Point> &rect) {
  if (rect.size() != 3)
    return; // 只支持3点补1点
  // 三点
  cv::Point A = rect[0], B = rect[1], C = rect[2];

  // 计算距离
  double dAB = cv::norm(A - B);
  double dAC = cv::norm(A - C);
  double dBC = cv::norm(B - C);

  // 找到对角线
  if (dAB > dAC && dAB > dBC) {
    // A-B为对角，C是邻点
    rect.push_back(A + B - C);
  } else if (dAC > dAB && dAC > dBC) {
    // A-C为对角，B是邻点
    rect.push_back(A + C - B);
  } else {
    // B-C为对角，A是邻点
    rect.push_back(B + C - A);
  }
}

int digitalDetect(cv::Mat &img, int size = 128) {
  cv::Mat resized, normalized;

  // 1. 输入检查
  if (img.empty()) {
    std::cerr << "Error: Input image is empty!" << std::endl;
    return -1;
  }

  // 2. 模型检查（OpenCV的dnn::Net使用empty()检查）
  if (net.empty()) {
    std::cerr << "Error: Model not loaded!" << std::endl;
    return -1;
  }

  // 转换为灰度图（如果是彩色输入）
  if (img.channels() > 1) {
    cv::cvtColor(img, resized, cv::COLOR_BGR2GRAY);
  } else {
    resized = img;
  }

  // 3. 转换为模型输入格式
  cv::Mat input_blob =
      cv::dnn::blobFromImage(resized, 1.0 / 255.0, cv::Size(28, 28),
                             cv::Scalar(0), false, false, CV_32F);

  // 4. 标准化（与训练一致）
  float mean = 0.1307f;
  float std = 0.3081f;
  input_blob = (input_blob - mean) / std;

  // 5. 推理
  net.setInput(input_blob);
  cv::Mat prob = net.forward();

  // 6. 概率归一化处理
  cv::Mat softmax_prob;
  cv::exp(prob, softmax_prob);          // 计算指数
  float sum = cv::sum(softmax_prob)[0]; // 计算总和
  softmax_prob = softmax_prob / sum;    // 归一化

  // 6. 后处理
  cv::Point class_id;
  double confidence;
  cv::minMaxLoc(softmax_prob, nullptr, &confidence, nullptr, &class_id);
  int predicted_digit = class_id.x;

  std::cout << "Predicted digit: " << predicted_digit
            << " with confidence: " << confidence << std::endl;

  // 7. 返回结果（可选：添加置信度阈值）
  const double CONFIDENCE_THRESHOLD = 0.5;
  if (confidence < CONFIDENCE_THRESHOLD) {
    std::cerr << "Warning: Low confidence prediction (" << confidence << ")"
              << std::endl;
    return -1; // 或者返回特定错误码
  }

  return predicted_digit;
}

int forwardStatic(cv::Mat &img, int size = 128, int times = 10) {
  std::vector<int> results;
  for (int i = 0; i < times; ++i) {
    results.emplace_back(digitalDetect(img, size));
  }

  std::unordered_map<int, int> freq;
  for (int digit : results) {
    freq[digit]++;
  }

  int max_digit = -1; // 默认返回-1（无效值）
  int max_count = 0;
  for (const auto &pair : freq) {
    if (pair.second > max_count) {
      max_count = pair.second;
      max_digit = pair.first;
    }
  }

  return max_digit;
}

cv::Mat getRectMat(const cv::Mat &img, const cv::RotatedRect &rotateRect,
                   int size = 128) {
  cv::Mat crop, gray;

  cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
  cv::threshold(gray, gray, 100, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);

  cv::Point2f vertices[4];
  cv::Size new_size(rotateRect.size.width * 0.9, rotateRect.size.height * 0.9);
  auto newRotateRect =
      cv::RotatedRect(rotateRect.center, new_size, rotateRect.angle);

  newRotateRect.points(vertices);
  std::vector<cv::Point2f> verticeVec = sortRectanglePoints(vertices);

  std::vector<cv::Point2f> rectVertices = {cv::Point(0, 0), cv::Point(size, 0),
                                           cv::Point(size, size),
                                           cv::Point(0, size)};

  auto P = cv::getPerspectiveTransform(verticeVec, rectVertices);
  cv::warpPerspective(gray, crop, P, cv::Size(size, size));

  return crop;
}

void completeHiddenRect(const vector<cv::Point> &approx, const int index1,
                        const int index2, vector<cv::Point> &rect) {
  if (index1 == -1 || index2 == -1)
    return;
  const int n = approx.size();
  rect.clear();

  // 获取相邻点索引（处理循环边界）
  int prev1 = (index1 - 1 + n) % n;
  int next1 = (index1 + 1) % n;
  int prev2 = (index2 - 1 + n) % n;
  int next2 = (index2 + 1) % n;

  // 计算两条边的方向
  cv::Point2f edge1_dir = approx[index1] - approx[prev1];
  cv::Point2f edge2_dir = approx[next2] - approx[index2];

  // 计算交点（隐藏点）
  cv::Point2f hidden1 =
      computeIntersection(approx[index1], edge1_dir, approx[index2], edge2_dir);

  // 计算另外两条边的方向
  cv::Point2f edge3_dir = approx[next1] - approx[index1];
  cv::Point2f edge4_dir = approx[prev2] - approx[index2];

  cv::Point2f hidden2 =
      computeIntersection(approx[index1], edge3_dir, approx[index2], edge4_dir);

  // 构建矩形
  rect.push_back(approx[index1]);
  rect.push_back(hidden1);
  rect.push_back(approx[index2]);
  rect.push_back(hidden2);

  ensureRectOrder(rect);
}

cv::Point2f computeIntersection(cv::Point2f p1, cv::Point2f dir1,
                                cv::Point2f p2, cv::Point2f dir2) {
  // 直线1: p1 + t*dir1
  // 直线2: p2 + s*dir2
  // 解方程组求交点

  float det = dir1.x * dir2.y - dir1.y * dir2.x;
  if (fabs(det) < 1e-5) {
    // 平行或重合，返回中点作为默认值
    return (p1 + p2) * 0.5f;
  }

  float t = ((p2.x - p1.x) * dir2.y - (p2.y - p1.y) * dir2.x) / det;
  return p1 + t * dir1;
}

// 确保矩形点是有序的（顺时针或逆时针）
void ensureRectOrder(vector<cv::Point> &rect) {
  if (rect.size() != 4)
    return;

  // 计算中心点（使用浮点运算）
  cv::Point2f center(0, 0);
  for (const auto &p : rect)
    center += cv::Point2f(p.x, p.y); // 显式转换为Point2f
  center *= 0.25f;

  // 确保顺时针顺序
  vector<pair<float, int>> angles;
  for (int i = 0; i < 4; ++i) {
    cv::Point2f vec(rect[i].x - center.x, // 分别计算x,y分量
                    rect[i].y - center.y);
    angles.emplace_back(atan2(vec.y, vec.x), i);
  }
  sort(angles.begin(), angles.end());

  vector<cv::Point> ordered_rect;
  for (const auto &a : angles) {
    ordered_rect.push_back(rect[a.second]);
  }
  rect = ordered_rect;
}